#ifndef CONTROL_H
#define CONTROL_H
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os.h"
#include "can.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "oled.h"
#include "oled_data.h"
#include "Caculate.h"
#include "DJI.h"
#include "STP23.h"
#include "wtr_can.h"
#include "delay_us.h"
#include "motor_mg513.h"
#include "math.h"
#include "delay_us.h"
#include "datou.h"
/*位置坐标数据*/
#define X1_store 210
#define X2_store 710
#define X3_store 1200
#define X4_placement_centre 710
#define Y1_store 82
#define Y2_store 500
#define Y3_placement_centre 2500
#define Z1_store -276
#define Z2_store -850
#define Z3_store -2100
#define Z4_store -2600
#define z_placement_1 -1000
#define z_placement_2 -620
#define z_placement_3 -1600
#define z_placement_4 -1200
#define z_placement_5 -2000
#define Y_placement_1 2788
#define Y_placement_2 3050

#define X_placement_a 215
#define Y_placement_a Y_placement_1
#define X_placement_b 67
#define Y_placement_b Y_placement_2
#define X_placement_c 497
#define Y_placement_c Y_placement_2
#define X_placement_d 914
#define Y_placement_d Y_placement_2
#define X_placement_e 1376
#define Y_placement_e Y_placement_2
#define X_placement_f 1236
#define Y_placement_f Y_placement_1
void x_position_control(float ref, DJI_t * motor);
void y_position_control(float ref, DJI_t * motor);
void z_position_control(float ref, DJI_t * motor);
void theta_position_control(float ref, DJI_t * motor);
void xyztheta_control(float ref_X,float ref_Y,float ref_z,float ref_theta,DJI_t *motor_x, DJI_t *motor_y,DJI_t *motor_z, DJI_t *motor_theta, CAN_HandleTypeDef *can_handle);
void claw2songkai(void);
void claw1songkai(void);
void claw1heshang(void);
void claw2heshang(void);
void claw_store_control(u8 storenum,u8 clawnum);
void claw_place_control(u8 storenum,u8 clawnum);
void claw_stacked_control(u8 storenum);

#endif // CONTROL_H